#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace std;
using namespace Eigen;
using namespace cv;

void drawRectangle(Mat &img, Point2i p1, Point2i p2, Point2i p3, Point2i p4, Scalar color, int thickness) {
    line(img, p1, p2, color, thickness, LINE_AA);
    line(img, p2, p3, color, thickness, LINE_AA);
    line(img, p3, p4, color, thickness, LINE_AA);
    line(img, p4, p1, color, thickness, LINE_AA);
}

Point2i cvtVector2Point(Vector2f vec) {
    Point2i p(int(vec(0, 0)), int(vec(1, 0)));
    return p;
}

int main(int argc, char const *argv[])
{
    Point2i lp1(100, 100), lp2(300, 100), lp3(300, 300), lp4(100, 300);
    Mat background = Mat(400, 800, CV_8UC3, Scalar(0, 0, 0));

    Rotation2Df pose1(0);
    Rotation2Df pose2(3.141592653);
    Vector2f rp1(-100, -100), loc1;
    Vector2f rp2(100, -100), loc2;
    Vector2f rp3(100, 100), loc3;
    Vector2f rp4(-100, 100), loc4;
    Vector2f center(600, 200);
    Rotation2Df pose;
    Matrix2f rot;

    int steps = 36;

    for (int i = 0; i < steps; ++i) {
        float ratio = i * 1.0 / steps;
        // 注意这里插值的规则。取的是逆时针或顺时针旋转角最小的那个方向。
        // 例如如果让他内插0到270度，他会选择逆时针旋转90度而不是顺时针旋转270度。
        // 因此如果这里直接设置终止为2pi，他会直接选择逆时针旋转0度，结果就是没有旋转。
        // 所以插值的直接上下限不能超过180度，否则就可能和你预计的方向相反了。后面可以通过系数t则可以扩展上限，从而达到2pi。
        // 例如0-60 t设为6，0-90 t设为4，0-180 
        // t设为2，这些都是等价的，且可以旋转360度
        pose = pose1.slerp(ratio*2, pose2);
        rot = pose.toRotationMatrix();
        cout << i << " pose : "<< pose.angle() * 180.0/CV_PI << endl;

        loc1 = rot * rp1 + center;
        loc2 = rot * rp2 + center;
        loc3 = rot * rp3 + center;
        loc4 = rot * rp4 + center;

        drawRectangle(background, lp1, lp2, lp3, lp4, Scalar(0, 0, 255), 2);
        drawRectangle(background,
                          cvtVector2Point(loc1),
                          cvtVector2Point(loc2),
                          cvtVector2Point(loc3),
                          cvtVector2Point(loc4),
                          Scalar(255, 255, 255), 2);
        circle(background, cvtVector2Point(loc1), 4, Scalar(255, 0, 0), -1, LINE_AA);
        circle(background, cvtVector2Point(loc2), 4, Scalar(0, 255, 0), -1, LINE_AA);
        circle(background, cvtVector2Point(loc3), 4, Scalar(0, 0, 255), -1, LINE_AA);
        circle(background, cvtVector2Point(loc4), 4, Scalar(0, 255, 255), -1, LINE_AA);

        imshow("img", background);
        if('q' == waitKey()) {
            break;
        }

        background = Scalar(0, 0, 0);
    } // for i
    return 0;
}
